/**
 * File: robot_sensor_util.cpp
 * Brief: 
 * Created by: zhangping
 * Created at: 2017.07.17
 * Modified by:
 * Modified at:
 **/

#include "robot_sensor_util.h"
#include "sensor_msgs/LaserScan.h"
#include "geometry_msgs/Quaternion.h"
#include "protocol.h"
#include "geomagnetic.h"
#include <iomanip>

namespace robot_node {


void do_geomagnetic(ros::Publisher &pub, const SensorData &data){
    // Valid data
    if(!IsValidGeomag(data.geomag)){
        const GeomagneticData &geomag = data.geomag;
        /*
        unsigned char cal_sum =  geomag.roll_h  + geomag.roll_l
                                +geomag.pitch_h + geomag.pitch_l
                                +geomag.yaw_h   + geomag.yaw_l
                                +geomag.t_h     + geomag.t_l;
        printf("Error:find a invalid geomagnetic sensor data,so abort it. [%x %x %x %x %x %x %x %x] sum=%x, cal_sum=%x\n"
            ,geomag.roll_h , geomag.roll_l
            ,geomag.pitch_h, geomag.pitch_l
            ,geomag.yaw_h  , geomag.yaw_l
            ,geomag.t_h    , geomag.t_l
            ,geomag.sum    , cal_sum);*/
        
        std::cerr<<"Error:find a invalid geomagnetic sensor data,so abort it. ["
                 <<geomag.roll_h <<"," <<geomag.roll_l<<","
                 <<geomag.pitch_h<<"," <<geomag.pitch_l<<","
                 <<geomag.yaw_h  <<"," <<geomag.yaw_l<<","
                 <<geomag.t_h    <<"," <<geomag.t_l<<"]"
                 <<std::endl;
        return;
    }

    // Build message
    geometry_msgs::Quaternion quat;
    if( !GeomagToQuaternion( data.geomag, quat) ){
        return;
    }

    // Publish message
    pub.publish(quat);	
}


void* SensorThread::_thread_handler(void *this_)
{

    SensorThread *obj = (SensorThread *)this_;

    ros::Publisher laser_pub = obj->node_.advertise<sensor_msgs::LaserScan>("robot/laser/scan", 50);
    ros::Publisher geomag_pub = obj->node_.advertise<geometry_msgs::Quaternion>("robot/sensor/Quaternion", 50);

    while(ros::ok()){
        /* Receive data from robot */
        U8 type;
        SensorData sdata;
        if(!obj->com_.ReceiveData(type, sdata)){
            std::cerr<<"Error:Receive sensor data failed"<<std::endl;
            continue;
        }
        
        /* Foward to the topic */
        if(type==kTypeUltr){  // Ultrasonic data
            // ...
        }else if(type==kTypeGyro){ //
            // ...
        }else if(type==kTypeGeomag){ // Geomagnetic Sensor Data
            do_geomagnetic( geomag_pub, sdata);
        }else if(type==kTypePos){ // Position data
            // ...
        }
    }
}


SensorThread::SensorThread(ros::NodeHandle &n, Communication &com)
:node_(n),com_(com)
{
    pthread_t tid=-1;
    if(0!=pthread_create(&tid, NULL, this->_thread_handler, this)){
        std::cout<<"error: falied at @pthread_create."<<std::endl;
            this->_clean_threads();
    }
    this->tid_ = tid;
}

void SensorThread::_clean_threads(){
    pthread_cancel(this->tid_);
}

SensorThread::~SensorThread()
{
    this->_clean_threads();
}

pthread_t SensorThread::GetPid()
{
    return this->tid_;
}

} // End of namespace

